#ifndef STEP_MOTOR_H //预编译指令，防止重复定义类
#define STEP_MOTOR_H

#define KP 0.15
#define KI 0.0
#define KD 0.0
#define DT 10  // ms

class PID_CONROLLER{
    public:
        PID_CONROLLER(float, float, float, float);
        float step(double target);
        void reset();
        float kp, ki, kd, dt;
        double output;
    private:
        double integral, derivative, error;
};

PID_CONROLLER :: PID_CONROLLER(float kp, float ki, float kd, float dt){
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
    this->dt = dt;
    this->reset();
}

float PID_CONROLLER :: step(double target){
    double now_error = target - this->output;   // 计算误差
    this->integral += this->error * this->dt;  // 计算误差的积分
    this->derivative = (now_error - this->error) / this->dt;  // 计算误差的微分
    this->output = this->output + (this->kp * now_error + this->ki * this->integral + this->kd * this->derivative);  // 更新状态
    this->error = now_error;  // 更新误差
    // Serial.printf("%.2f, %.2f, %.2f, %.2f, %.2f ::::",target, this->integral, this->derivative, this->output, this->error);
    return this->output;
}


void PID_CONROLLER :: reset(){
    this->integral = 0.;
    this->derivative = 0.;
    this->output = 0.;
    this->error = 0.;
}

class StepMotor{
    public:
        StepMotor(uint8_t, uint8_t, uint8_t); //类的构造函数，与类名相同
        void motorSpeed(double);
        void motorSpeedPID(double);
        void strokeInit(uint8_t, uint8_t, uint8_t);
        void updateDistance(float, bool);
        void loop_strokeChecker();
        void loop_runner();
        uint8_t dirState;
        double  nowFreqAbs;
        double  freqSpeed;
        double  targetSpeed;
        int64_t addr = 0;
        int64_t nowDistance=0;
        uint8_t positivePinValue = 0;
        uint8_t negativePinValue = 0;
        PID_CONROLLER * pid_conroller;
    private:
        //-------------电机与PWM通道所使用的变量-------------------
        uint8_t motorChannel;
        uint8_t dirPin;
        uint8_t pulPin;
        //--------------为限位开关功能使用的变量--------------------
        uint8_t positivePin = 255;
        uint8_t negativePin = 255;
        uint8_t strokeOnPinMode = HIGH;  // 限位开关为“触发”时这个管脚的状态
        //---------------为计时器更新位置使用的变量-----------------
        float dir_ = -1;              // 方向标识符
        int   dx_  =  0;              // 时间微分
};

StepMotor :: StepMotor(uint8_t Pul, uint8_t Dir, uint8_t motorChannel){
    // 初始化一个电机，提供其关联管脚与电机号
    PID_CONROLLER *pid_conroller = new PID_CONROLLER(KP, KI, KD, ((float)DT)/1000.);
    this->pid_conroller = pid_conroller;
    this->motorChannel = 2*motorChannel;
    this->dirPin = Dir;
    this->pulPin = Pul;
    pinMode(dirPin, OUTPUT);
    digitalWrite(dirPin, LOW);
    // ledcSetup(this->motorChannel, 0, 10);
    ledcSetup(this->motorChannel,1,10);
    ledcWriteTone(this->motorChannel,0);
    ledcAttachPin(Pul, this->motorChannel);
}

void StepMotor :: loop_runner(){
    TickType_t xLastWakeTime = xTaskGetTickCount();
    const TickType_t xFrequency = DT;
    for(;;){
        vTaskDelayUntil(&xLastWakeTime, xFrequency);
        float old_output = this->pid_conroller->output;
        float output = this->pid_conroller->step(this->targetSpeed);
        // Serial.printf("%.2f\t%.2f\n", old_output, output);
        this->motorSpeed(output);
    }
}

void StepMotor :: motorSpeedPID(double targetSpeed){
    this->targetSpeed = targetSpeed;
}

void StepMotor :: motorSpeed(double freqSpeed){
    // 设置电机的速度
    // 只有没有触发限位时才有效
    if(freqSpeed>0 && positivePinValue!=strokeOnPinMode){
        digitalWrite(dirPin, HIGH);
        this->dirState = HIGH;
        this->nowFreqAbs = abs(freqSpeed);
        this->freqSpeed = freqSpeed;
        ledcWriteTone(motorChannel, nowFreqAbs);
    }
    else if (freqSpeed<0 && negativePinValue!=strokeOnPinMode){
        digitalWrite(dirPin, LOW);
        this->dirState = LOW;
        this->nowFreqAbs = abs(freqSpeed);
        this->freqSpeed = freqSpeed;
        ledcWriteTone(motorChannel, nowFreqAbs);
    }
    if (freqSpeed < 1e-3 && freqSpeed > -1e-3){
        this->nowFreqAbs = 0.;
        this->freqSpeed = 0.;
        ledcWriteTone(motorChannel, 0);
    }
}

void StepMotor :: strokeInit(uint8_t positivePin, uint8_t negativePin, uint8_t strokeOnPinMode=HIGH){
    // 初始化限位开关
    if (strokeOnPinMode==LOW){
        pinMode(positivePin, INPUT_PULLUP);
        pinMode(negativePin, INPUT_PULLUP);
    }else{
        pinMode(positivePin, INPUT_PULLDOWN);
        pinMode(negativePin, INPUT_PULLDOWN);
    }
    this->positivePinValue = digitalRead(positivePin);
    this->negativePinValue = digitalRead(negativePin);
    this->positivePin = positivePin;
    this->negativePin = negativePin;
    this->strokeOnPinMode = strokeOnPinMode;
}

void StepMotor :: loop_strokeChecker(){
    // 把这个放在主循环里来做限位检查
    if (positivePin != 255){  // 只有管脚合法初始化才会启用
        uint8_t now_positive = !strokeOnPinMode;
        uint8_t now_negative = !strokeOnPinMode;
        // 信号滤波
        if(digitalRead(positivePin) == strokeOnPinMode){
            delay(1);
            if(digitalRead(positivePin)==strokeOnPinMode){
                now_positive = strokeOnPinMode;
            }
        }
        if(digitalRead(negativePin) == strokeOnPinMode){
            delay(1);
            if(digitalRead(negativePin)==strokeOnPinMode){
                now_negative = strokeOnPinMode;
            }
        }

        if(positivePinValue!=strokeOnPinMode && now_positive==strokeOnPinMode){
            // 开启沿触发
            if(nowFreqAbs!=0.0 && dirState){
                ledcWriteTone(motorChannel, 0.0);
                this->nowFreqAbs = 0.0;
            }
        }

        if(negativePinValue!=strokeOnPinMode && now_negative==strokeOnPinMode){
            // 开启沿触发
            if(nowFreqAbs!=0.0 && !dirState){
                ledcWriteTone(motorChannel, 0.0);
                this->nowFreqAbs = 0.0;
            }
        }
        // 更新限位状态
        this->positivePinValue = now_positive;
        this->negativePinValue = now_negative;
    }    
}

void StepMotor :: updateDistance(float dt, bool debug){
    dir_ = -1;
    if(this->dirState){
        dir_ = 1;
    }
    // 结果为整形，但是为了提升精度：100,005代表100.005，表示的脉冲数附带了小数点后三位的结果。
    dx_ = (int)(this->nowFreqAbs * dt * 1000 * dir_);
    this->nowDistance = this->nowDistance + dx_;
    if (debug){
        Serial.printf("%.2f, %d, %f, %.2f, %.2f\n", dx_, this->nowDistance, dir_, dt, this->nowFreqAbs);
    }
}
#endif